/*
    Copyright 2006 Andrew Wilkinson <andrew@indiegigs.co.uk>

    This file is part of libfixed.

    libfixed is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    libfixed is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with libfixed; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include <stdio.h>
#include <stdlib.h>

#include "fixed.h"

Fixed16 Fixed_dtor16(Fixed16 f) {
    return Fixed_div16(Fixed_mul16(Fixed_pi16, f), Fixed_createi16(360));
}

Fixed32 Fixed_dtor32(Fixed32 f) {
    return Fixed_div32(Fixed_mul32(Fixed_pi32, f), Fixed_createi32(360));
}

//TODO: Should be replaced by sqrt(x) = e^(0.5*ln(x))
Fixed16 Fixed_sqrt16(Fixed16 value) {
    Fixed16 r = Fixed_pow16(Fixed_e16, Fixed_mul16(Fixed_half16, Fixed_log16(value)));
#ifdef DEBUG
        fprintf(stderr, "Fixed_sqrt16: %f. Result: %f Expected %f\n", Fixed_toFloat16(value), Fixed_toFloat16(r), sqrtf(Fixed_toFloat16(value)));
#endif
    return r;
}

Fixed32 Fixed_sqrt32(Fixed32 value) {
    return Fixed_pow32(Fixed_e32, Fixed_mul32(Fixed_half32, Fixed_log32(value)));
}

extern int32_t Fixed_sintable16[];

Fixed16 Fixed_sintablelookup16(Fixed16 angle) {
    Fixed16 i = Fixed_div16(Fixed_muli16(angle, 320*2), Fixed_pi16);

    return (Fixed16){Fixed_sintable16[Fixed_round16(i)]};
}

#define absf(x) ((x) < 0 ? -(x) : (x))

Fixed16 Fixed_sin16(Fixed16 angle) {
    Fixed16 r;
    if(Fixed_lt16(angle, Fixed_zero16)) {
        r = Fixed_sub16(Fixed_zero16, Fixed_sin16(Fixed_add16(angle, Fixed_pi16)));
    } else if(Fixed_gt16(angle, Fixed_pi16)) {
        r = Fixed_sub16(Fixed_zero16, Fixed_sin16(Fixed_sub16(angle, Fixed_pi16)));
    } else if(Fixed_gt16(angle, Fixed_divi16(Fixed_pi16, 2))) {
        r = Fixed_sin16(Fixed_sub16(Fixed_pi16, angle));
    } else {
        r = Fixed_sintablelookup16(angle);
    }
#ifdef DEBUG
    fprintf(stderr, "Fixed_sin16: %f. Result: %f Expected %f\n", Fixed_toFloat16(angle), Fixed_toFloat16(r), sinf(Fixed_toFloat16(angle)));
    if(absf(Fixed_toFloat16(r) - sinf(Fixed_toFloat16(angle))) > 0.01) {
        exit(1);
    }
#endif
    return r;
}

Fixed16 Fixed_cos16(Fixed16 angle) {
    Fixed16 r = Fixed_sin16(Fixed_add16(angle, Fixed_divi16(Fixed_pi16, 2)));
#ifdef DEBUG
        fprintf(stderr, "Fixed_cos16: %f. Result: %f Expected %f\n", Fixed_toFloat16(angle), Fixed_toFloat16(r), cosf(Fixed_toFloat16(angle)));
#endif
    return r;
}

/*
//TODO: This is quite inaccurate.
Fixed16 Fixed_sincos16(Fixed16 angle, unsigned char want_sine) {
    int N = 1024;
    int n;
    Fixed16 pi2 = Fixed_muli16(Fixed_pi16, 2);
    Fixed16 d = Fixed_divi16(Fixed_muli16(Fixed_pi16, 2), N);
    Fixed16 s = Fixed_zero16;
    Fixed16 c = Fixed_one16;
    Fixed16 sn;
    Fixed16 cn;
    while(Fixed_lt16(angle, Fixed_zero16)) {
        angle = Fixed_add16(angle, pi2);
    }
    while(Fixed_ge16(angle, pi2)) {
        angle = Fixed_sub16(angle, pi2);
    }
    for(n = Fixed_round16(Fixed_muli16(Fixed_div16(angle, pi2), N)); n > 0; n--) {
        sn = s; cn = c;
        s = Fixed_add16(sn, Fixed_mul16(d, cn));
        c = Fixed_sub16(cn, Fixed_mul16(d, sn));
    }
    if(want_sine) { return s; } else { return c; }
}

Fixed16 Fixed_sin16(Fixed16 angle) {
    Fixed16 r = Fixed_sincos16(angle, 1);
#ifdef DEBUG
        fprintf(stderr, "Fixed_sin16: %f. Result: %f Expected %f\n", Fixed_toFloat16(angle), Fixed_toFloat16(r), sinf(Fixed_toFloat16(angle)));
#endif
    return r;
}

Fixed16 Fixed_cos16(Fixed16 angle) {
    Fixed16 r = Fixed_sincos16(angle, 0);
#ifdef DEBUG
        fprintf(stderr, "Fixed_cos16: %f. Result: %f Expected %f\n", Fixed_toFloat16(angle), Fixed_toFloat16(r), cosf(Fixed_toFloat16(angle)));
#endif
    return r;
}*/

//TODO: This is quite inaccurate.
Fixed32 Fixed_sincos32(Fixed32 angle, unsigned char want_sine) {
    int N = 1024;
    int n;
    Fixed32 pi2 = Fixed_muli32(Fixed_pi32, 2);
    Fixed32 d = Fixed_divi32(Fixed_muli32(Fixed_pi32, 2), N);
    Fixed32 s = Fixed_zero32;
    Fixed32 c = Fixed_one32;
    Fixed32 sn;
    Fixed32 cn;
    while(Fixed_lt32(angle, Fixed_zero32)) {
        angle = Fixed_add32(angle, pi2);
    }
    while(Fixed_ge32(angle, pi2)) {
        angle = Fixed_sub32(angle, pi2);
    }
    for(n = Fixed_round32(Fixed_muli32(Fixed_div32(angle, pi2), N)); n > 0; n--) {
        sn = s; cn = c;
        s = Fixed_add32(sn, Fixed_mul32(d, cn));
        c = Fixed_sub32(cn, Fixed_mul32(d, sn));
    }
    if(want_sine) { return s; } else { return c; }
}

Fixed32 Fixed_sin32(Fixed32 angle) {
    return Fixed_sincos32(angle, 1);
}

Fixed32 Fixed_cos32(Fixed32 angle) {
    return Fixed_sincos32(angle, 0);
}

